﻿// RiedmaierWorldCoordinateReconstruction
// Object representing functionality of Riedmaier coordinate reconstruction algorithm 
// for reconstructing 3D coordinates from 2D image plane coordinates
// 
// FLMScan - Featured by FabLab Munich

using System;
using FLMScan.Geometry;

namespace FLMScan.WorldCoordinateReconstruction.Riedmaier
{
    class RiedmaierWorldCoordinateReconstruction 
    {
        private LaserPlane laserPlane;
        private CameraSensor cameraSensor;

        public RiedmaierWorldCoordinateReconstruction(LaserPlane p, CameraSensor sensor)
        {
            laserPlane = p;
            cameraSensor = sensor;
        }

        public FlmPoint reconstruct(FlmPoint imagePlanePoint)
        {
            // calculate s (parameter for line of sight)
            double s = (-laserPlane.D) /
                ( (laserPlane.A * imagePlanePoint.X) 
                + (laserPlane.B * imagePlanePoint.Y) 
                + laserPlane.C * cameraSensor.FocalLengthInMillimetersX );

            LineOfSight lineOfSight = new LineOfSight( cameraSensor.FocalLengthInMillimetersX );
            FlmPoint worldCoordinatePoint = lineOfSight.getPointOnLine( s, imagePlanePoint );

            return worldCoordinatePoint;
        }
    }
}
